#include "motion.hpp"
#include "posinfo.hpp"
#include <cmath>

Motion::Motion()
{
    spt.SetTimeout(10.0f);
    spt.Init();
}

Motion::~Motion()
{
}

void Motion::move(float vx, float vy, float vyaw)
{
    if (vx < -2.5) {
        vx = -2.5;
    }

    if (vx > 5) {
        vx = 5;
    }

    if (vy < -2.5) {
        vy = -2.5;
    }

    if (vy > 5) {
        vy = 5;
    }

    if (vyaw < -4) {
        vyaw = -4;
    }

    if (vyaw > 4) {
        vyaw = 4;
    }

    int ret = spt.Move(vx, vy, vyaw);
    if (ret) {
        std::cout << "err: sport.Move(), ret = " << ret << std::endl;
        return;
    }
}

void Motion::movestop()
{
    int ret = spt.StopMove();
    if (ret) {
        std::cout << "err: sport.StopMove(), ret = " << ret << std::endl;
        return;
    }
}

void Motion::sport_position(float x, float y, float yaw)
{
    float lin_vel_thr = 1.0;
    bool run = true;

    while (run) {
        /* 获取当前机器狗位置 */
        bool valid = false;
        float curr_x = 0.0f;
        float curr_y = 0.0f;
        float curr_yaw = 0.0f;
        PosInfo::get(valid, curr_x, curr_y, curr_yaw);
        if (!valid) {
            msleep(100);
            continue;
        }

        /* 计算目标位置和当前位置距离和偏差角度 */
        float dx = x - curr_x;
        float dy = y - curr_y;
        float distance = std::sqrt(dx * dx + dy * dy);
        float target_yaw = atan2(dy, dx);
        float dyaw = target_yaw - curr_yaw;

        /* 由于反正切的结果不唯一，防止机器人自身多次转圈，限制角度差在 [-pi, pi] 范围内 */
        while (dyaw > PI) {
            dyaw = dyaw - 2 * PI;
        }
        while (dyaw < -PI) {
            dyaw = dyaw + 2 * PI;
        }

        /* 调试信息 */
        // std::cout << "dx         : " << dx << std::endl;
        // std::cout << "dy         : " << dy << std::endl;
        // std::cout << "distance   : " << distance << std::endl;
        // std::cout << "curr_yaw   : " << curr_yaw << std::endl;
        // std::cout << "target_yaw : " << target_yaw << std::endl;
        // std::cout << "dyaw       : " << dyaw << std::endl;

        /* 根据角度偏差, 计算角速度 */
        float ang_vel = dyaw * 1.0;
        if (ang_vel > max_ang_vel) {
            ang_vel = max_ang_vel;
        } else if (ang_vel < -max_ang_vel) {
            ang_vel = -max_ang_vel;
        }

        /* 角度纠正后, 沿直线前进 */
        float lin_vel = distance * 0.5;

        /* 角度偏差比较大的时候, 限制线速度, 缩小转弯半径*/
        if (std::fabs(dyaw) > ang_thr) {
            lin_vel_thr = low_lin_vel;
        } else {
            lin_vel_thr = max_lin_vel;
        }

        if (lin_vel > lin_vel_thr) {
            lin_vel = lin_vel_thr;
        } else if (lin_vel < -lin_vel_thr) {
            lin_vel = -lin_vel_thr;
        }

        /* 距离在阈值范围内，说明到达了目标位置 */
        if (distance < dist_thr) {
            movestop();
            break;
        }

        // std::cout << "ang_vel: " << ang_vel << std::endl;
        // std::cout << "lin_vel: " << lin_vel << std::endl;

        move(lin_vel, 0, ang_vel);
        msleep(100);
    }
}

void Motion::lin_Vel_set(float vel)
{
    max_lin_vel = vel;
    std::cout << "max_lin_vel: " << max_lin_vel << std::endl;
}

void Motion::msleep(int ms)
{
    usleep(ms * 1000);
}